# coding:utf-8
import RPi.GPIO as GPIO
import time
from smbus import SMBus

from socket import  *

from ctypes import cdll

# import ctypes
# so = ctypes.CDLL('./_XiaoRGEEK_SERVO_.so')

# cur = cdll.LoadLibrary('./_XiaoRGEEK_SERVO_.so')
from _XiaoRGEEK_SERVO_ import XR_Servo

class CarOperation:
    def __init__(self):
        # self.__tcp_server =  socket(AF_INET, SOCK_STREAM)
        self.__init_car_pin()

        self.__XRservo = SMBus(1)

    def __init_car_pin(self):
        GPIO.setmode(GPIO.BCM)

        ########LED口定义#################
        self.__LED0 = 10
        self.__LED1 = 9
        self.__LED2 = 25

        ########电机驱动接口定义#################
        self.__ENA = 13  # //L298使能A
        self.__ENB = 20  # //L298使能B
        self.__IN1 = 19  # //电机接口1
        self.__IN2 = 16  # //电机接口2
        self.__IN3 = 21  # //电机接口3
        self.__IN4 = 26  # //电机接口4

        ########舵机接口定义#################

        ########超声波接口定义#################
        self.__ECHO = 4  # 超声波接收脚位
        self.__TRIG = 17  # 超声波发射脚位

        #######################################
        #########管脚类型设置及初始化##########
        #######################################
        GPIO.setwarnings(False)

        #########led初始化为000##########
        GPIO.setup(self.__LED0, GPIO.OUT, initial=GPIO.HIGH)
        GPIO.setup(self.__LED1, GPIO.OUT, initial=GPIO.HIGH)
        GPIO.setup(self.__LED2, GPIO.OUT, initial=GPIO.HIGH)

        #########电机初始化为LOW##########
        GPIO.setup(self.__ENA, GPIO.OUT, initial=GPIO.LOW)
        ENA_pwm = GPIO.PWM(self.__ENA, 1000)
        ENA_pwm.start(0)
        ENA_pwm.ChangeDutyCycle(100)
        GPIO.setup(self.__IN1, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(self.__IN2, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(self.__ENB, GPIO.OUT, initial=GPIO.LOW)
        ENB_pwm = GPIO.PWM(self.__ENB, 1000)
        ENB_pwm.start(0)
        ENB_pwm.ChangeDutyCycle(100)
        GPIO.setup(self.__IN3, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(self.__IN4, GPIO.OUT, initial=GPIO.LOW)

        ##########超声波模块管脚类型设置#########
        GPIO.setup(self.__TRIG, GPIO.OUT, initial=GPIO.LOW)  # 超声波模块发射端管脚设置trig
        GPIO.setup(self.__ECHO, GPIO.IN, pull_up_down=GPIO.PUD_UP)  # 超声波模块接收端管脚设置echo

        self.Servo = XR_Servo()

    ##########机器人方向控制###########################
    def Motor_Forward(self):
        # print 'motor forward'
        GPIO.output(self.__ENA, True)
        GPIO.output(self.__ENB, True)
        GPIO.output(self.__IN1, True)
        GPIO.output(self.__IN2, False)
        GPIO.output(self.__IN3, True)
        GPIO.output(self.__IN4, False)
        GPIO.output(self.__LED1, False)  # LED1亮
        GPIO.output(self.__LED2, False)  # LED1亮

    def Motor_Backward(self):
        # print 'motor_backward'
        GPIO.output(self.__ENA, True)
        GPIO.output(self.__ENB, True)
        GPIO.output(self.__IN1, False)
        GPIO.output(self.__IN2, True)
        GPIO.output(self.__IN3, False)
        GPIO.output(self.__IN4, True)
        GPIO.output(self.__LED1, True)  # LED1灭
        GPIO.output(self.__LED2, False)  # LED2亮

    def Motor_TurnLeft(self):
        # print 'motor_turnleft'
        GPIO.output(self.__ENA, True)
        GPIO.output(self.__ENB, True)
        GPIO.output(self.__IN1, True)
        GPIO.output(self.__IN2, False)
        GPIO.output(self.__IN3, False)
        GPIO.output(self.__IN4, True)
        GPIO.output(self.__LED1, False)  # LED1亮
        GPIO.output(self.__LED2, True)  # LED2灭

    def Motor_TurnRight(self):
        # print 'motor_turnright'
        GPIO.output(self.__ENA, True)
        GPIO.output(self.__ENB, True)
        GPIO.output(self.__IN1, False)
        GPIO.output(self.__IN2, True)
        GPIO.output(self.__IN3, True)
        GPIO.output(self.__IN4, False)
        GPIO.output(self.__LED1, False)  # LED1亮
        GPIO.output(self.__LED2, True)  # LED2灭

    def Motor_Stop(self):
        GPIO.output(self.__ENA, False)
        GPIO.output(self.__ENB, False)
        GPIO.output(self.__IN1, False)
        GPIO.output(self.__IN2, False)
        GPIO.output(self.__IN3, False)
        GPIO.output(self.__IN4, False)
        GPIO.output(self.__LED1, True)  # LED1灭
        GPIO.output(self.__LED2, True)  # LED2亮

    ####################################################
    ##函数名称 ：Get_Distence()
    ##函数功能 超声波测距，返回距离（单位是厘米）
    ####################################################
    def Get_Distence(self):
        time.sleep(0.01)
        GPIO.output(self.__TRIG, GPIO.HIGH)
        time.sleep(0.000015)
        GPIO.output(self.__TRIG, GPIO.LOW)
        while not GPIO.input(self.__ECHO):
            pass
        t1 = time.time()
        while GPIO.input(self.__ECHO):
            pass
        t2 = time.time()
        Distence = (t2 - t1) * 340 / 2 * 100
        time.sleep(0.01)
        return Distence

    ####################################################
    ##函数名称 Avoid_wave()
    ##函数功能 超声波避障函数
    ####################################################
    def Avoid_wave(self):
        dis = self.Get_Distence()
        print(dis)
        if  dis>25:
            self.Motor_Forward()
        else:
            self.Motor_Stop()
            self.Motor_TurnLeft()
            # self.Avoid_wave()


    ##函数功能 ：舵机控制函数
    ##入口参数 ：ServoNum(舵机号)，angle_from_protocol(舵机角度)
    ##出口参数 ：无
    ####################################################
    def Angle_cal(self, angle_from_protocol):
        angle = hex(eval('0x' + angle_from_protocol))
        angle = int(angle, 16)
        if angle > 160:
            angle = 160
        elif angle < 15:
            angle = 15
        return angle

    def SetServoAngle(self, motor_number: int ,angle_from_protocol: int):
        # GPIO.output(self.__LED0, False)
        # # GPIO.output(self.__LED1, True)
        # GPIO.output(self.__LED2, False)
        # time.sleep(1)
        # GPIO.output(self.__LED0, True)
        # # GPIO.output(self.__LED1, True)
        # GPIO.output(self.__LED2, True)

        print(motor_number, angle_from_protocol)
        # Servo = XR_Servo()
        # Servo.XiaoRGEEK_ReSetServo()

        self.Servo.XiaoRGEEK_SetServoAngle(motor_number, angle_from_protocol)  # 设置1号舵机角度为30度

    def cpu_temp(self):
        file = open("/sys/class/thermal/thermal_zone0/temp")    # 打开文件
        temp = float(file.read()) / 1000     # 读取结果，并转换为浮点数
        file.close()    #关闭文件
        print("temp:" ,temp) # 向终端控制台打印
        if temp <60:
            GPIO.output(self.__LED0, True)
            # GPIO.output(self.__LED1, True)
            GPIO.output(self.__LED2, True)
        else:
            GPIO.output(self.__LED0, False)
            # GPIO.output(self.__LED1, True)
            GPIO.output(self.__LED2, False)
        return temp